#include<iostream>
#include <iomanip>
#include <opencv.hpp>
using namespace cv;
using namespace std;

#define GRID_CONT_EMPTY 0
#define GRIDMAP_CONT_BLOCKED 9
#define GRIDMAP_CONT_START 1
#define GRIDMAP_CONT_END 2
class Grid
{
public:
	int content;
	int distance;
	//char tochar;
	Grid(){
		distance=999;
		content=GRID_CONT_EMPTY;
		//tochar='^';
	}
};

class GridMap
{

public:
	int weight,height;
	Grid **map;
	GridMap(int weight,int height){
		map=new Grid*[weight];
		for(int i=0;i<weight;i++){
			map[i]=new Grid[height];
		}
		this->weight=weight;
		this->height=height;
	}
	~GridMap(){
		delete map;
	}
	void setGrid(int x,int y, int content){
		map[x][y].content=content;
	}
	void initDistance(){
		for(int i=0;i<weight;i++)
		{
			for(int j=0;j<height;j++)
			{
				if(map[i][j].content==GRID_CONT_EMPTY||
					map[i][j].content==GRIDMAP_CONT_BLOCKED||
					map[i][j].content==GRIDMAP_CONT_START)map[i][j].distance=999;
				if(map[i][j].content==GRIDMAP_CONT_END)map[i][j].distance=0;
			}
		}
	}

	void printContent(){
		for(int i=0;i<height;i++)
		{
			for(int j=0;j<weight;j++)
			{
				if(map[j][i].content==GRID_CONT_EMPTY)cout<<"^";
				if(map[j][i].content==GRIDMAP_CONT_BLOCKED)cout<<"X";
				if(map[j][i].content==GRIDMAP_CONT_START)cout<<"S";
				if(map[j][i].content==GRIDMAP_CONT_END)cout<<"E";

			}
			cout<<endl;
		}
		cout<<endl;
	}

	void printDistance(){

		for(int i=0;i<height;i++)
		{
			for(int j=0;j<weight;j++)
			{
				cout<<setw(5);
				cout<<map[j][i].distance;
			}
			cout<<endl;
		}
		cout<<endl;
	}

	void findPath(){
		if(!isValidMap())return;
		for(int i=0;i<weight;i++){
			for(int j=0;j<height;j++){
				if(map[i][j].content==GRIDMAP_CONT_END)map[i][j].distance=0;
			}
		}


		while(1){
			bool makeprogress=false;
			for(int i=0;i<weight;i++)
			{
				for(int j=0;j<height;j++)
				{
					if(map[i][j].content!=GRIDMAP_CONT_BLOCKED)
					{
						for(int k=0;k<4;k++){
							int x=i;int y=j;
							switch (k)
							{
							case 0 :
								x+=1;y+=0;
								break;
							case 1 :
								x+=(-1);y+=0;
								break;
							case 2 :
								x+=0;y+=1;
								break;
							case 3 :
								x+=0;y+=(-1);
								break;
							}
							if(x==weight||y==height||x<0||y<0)continue;
							if(map[x][y].content==GRIDMAP_CONT_BLOCKED)continue;
							if(map[x][y].distance>map[i][j].distance+1){
								map[x][y].distance=map[i][j].distance+1;
								makeprogress=true;
							}

						}
					}

				}
			}
			if (!makeprogress)
			{
				break;
			}
		}
	}

	void getPath(std::vector<CvPoint> &path){
		if(!isValidMap())return;
		int prev_x,prev_y;
		for(int i=0;i<weight;i++){
			for(int j=0;j<height;j++){
				if(map[i][j].content==GRIDMAP_CONT_START){
					prev_x=i;prev_y=j;
				}
			}
		}
		path.push_back(cvPoint(prev_x,prev_y));
		bool reach=false;
		while(!reach){
			int shortest_distance=999;
			CvPoint shortest_grid;
			for(int k=0;k<4;k++){
				int x=prev_x,y=prev_y;
				switch (k)
				{
				case 0 :
					x+=0;y+=(-1);
					break;
				case 1 :
					x+=1;y+=0;
					break;
				case 2 :
					x+=0;y+=1;
					break;
				case 3 :
					x+=(-1);y+=0;
					break;
				}
				if(x==weight||y==height||x<0||y<0)continue;
				if(map[x][y].content==GRIDMAP_CONT_BLOCKED)continue;
				if(map[x][y].distance<shortest_distance)
				{
					shortest_distance=map[x][y].distance;
					shortest_grid.x=x;shortest_grid.y=y;
				}
			}
			prev_x=shortest_grid.x;prev_y=shortest_grid.y;
			path.push_back(shortest_grid);
			if (map[shortest_grid.x][shortest_grid.y].distance==0)
			{
				reach=true;
			}
		}
	}

	CvPoint closestGrid(CvPoint pixelpoint){
		return cvPoint(pixelpoint.x/100,pixelpoint.y/100);
	}

	bool isValidMap(){
		bool start=false,end=false;
		for(int i=0;i<weight;i++)
		{
			for(int j=0;j<height;j++)
			{
				if(map[i][j].content==GRIDMAP_CONT_START)start=true;
				if(map[i][j].content==GRIDMAP_CONT_END)end=true;
			}
		}
		return start&&end;
	}




};